package ncsutest.robowulf;

import com.sun.spot.resourcesharing.ResourceUnavailableException;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IIOPin;
import com.sun.spot.sensorboard.io.IInputPin;
import com.sun.spot.sensorboard.io.IOutputPin;
import com.sun.spot.sensorboard.io.IScalarInput;
import com.sun.spot.util.Utils;

import ncsutest.NCSUTestBase;
import ncsutest.robowulf.sensors.IRSensor;
import ncsutest.robowulf.sensors.IRSensorListener;
import ncsutest.robowulf.sensors.MaxSonarEZ1UltrasoundSensor;
import ncsutest.robowulf.sensors.PINGUltrasoundSensor;

public class RobowulfTests extends NCSUTestBase {
	private static final IScalarInput[] scalarInputs = EDemoBoard.getInstance().getScalarInputs();
	private static final IIOPin[] digitalInputs = EDemoBoard.getInstance().getIOPins();
	private static final IOutputPin[] outputs = EDemoBoard.getInstance().getOutputPins();

	protected static final IScalarInput[] irSensorPins = {scalarInputs[0], scalarInputs[2]};
	protected static final IOutputPin[] servoPins = {outputs[0], outputs[1]};

	protected static final IIOPin singleSonarPin = digitalInputs[0];
	protected static final IIOPin[] tripleSonarPins = {digitalInputs[0], digitalInputs[1], digitalInputs[2]};

	public RobowulfTests(int[] testsToRun) {
		super(testsToRun);
	}

	/**
	 * Test00: tests single sonar sensor
	 */
	public void test00() {
		if (!testSonarOnPin(singleSonarPin))
			fail("No sonar sensor detected (both MaxSonarEZ1 and PING))) failed to respond). Ensure that the sonar is plugged into the correct pin, that the battery pack is turned on, and that the batteries are charged.");
	}
	
	/**
	 * Test01: tests first (left) sonar sensor in triple sensor setup
	 */
	public void test01() {
		if (!testSonarOnPin(tripleSonarPins[0]))
			fail("No left sonar sensor detected (both MaxSonarEZ1 and PING))) failed to respond). Ensure that the sonar is plugged into the correct pin, that the battery pack is turned on, and that the batteries are charged.");
	}

	/**
	 * Test02: tests second (center) sonar sensor in triple sensor setup
	 */
	public void test02() {
		if (!testSonarOnPin(tripleSonarPins[1]))
			fail("No center sonar sensor detected (both MaxSonarEZ1 and PING))) failed to respond). Ensure that the sonar is plugged into the correct pin, that the battery pack is turned on, and that the batteries are charged.");
	}

	/**
	 * Test03: tests third (right) sonar sensor in triple sensor setup
	 */
	public void test03() {
		if (!testSonarOnPin(tripleSonarPins[2]))
			fail("No right sonar sensor detected (both MaxSonarEZ1 and PING))) failed to respond). Ensure that the sonar is plugged into the correct pin, that the battery pack is turned on, and that the batteries are charged.");
	}

	/**
	 * Test04: tests the left IR sensor alone
	 */
	public void test04() {
		if (!testIRSensor(servoPins[0], irSensorPins[0]))
			fail("The left IR sensor failed to detect wheel movement. Ensure that the IR sensor is plugged into the correct pin, that the battery pack is turned on, and that the batteries are charged.");
	}

	/**
	 * Test05: tests the right IR sensor alone
	 */
	public void test05() {
		if (!testIRSensor(servoPins[1], irSensorPins[1]))
			fail("The right IR sensor failed to detect wheel movement. Ensure that the IR sensor is plugged into the correct pin, that the battery pack is turned on, and that the batteries are charged.");
	}

	/**
	 * Test06: tests right IR sensor independence from left (e.g. it fails if the right IR sensor is affected when the left IR sensor changes)
	 */
	public void test06() {
		RobowulfServo servoLeft = new RobowulfServo(servoPins[0]);
		IRSensor irLeft = new IRSensor("Left IRSensor", irSensorPins[0]);
		IRSensor irRight = new IRSensor("Right IRSensor", irSensorPins[1]);
		
		servoLeft.setBounds(2000, 1000);
		
		int result;
		
		IRSensorTestHelper helperLeft = new IRSensorTestHelper(servoLeft, irLeft, irRight);
		result = helperLeft.doTest(10, 5000);

		servoLeft.destroy();
		irLeft.destroy();
		irRight.destroy();
		
		assertFalse("The left IR sensor did not respond to wheel rotation. Ensure that the IR sensor is plugged into the correct pin, that the battery pack is turned on, and that the batteries are charged.",
					result == IRSensorTestHelper.FAILED_TIMEOUT);

		assertFalse("The right IR sensor's readings were affected by the left IR sensor. This is not supposed to happen, and may be the result of more than 3V being applied to the analog pin(s)",
					result == IRSensorTestHelper.FAILED_SENSOR_SWING || result == IRSensorTestHelper.FAILED_SENSOR_COUNTED);
	}
	
	/**
	 * Test07: tests left IR sensor independence from right (e.g. it fails if the left IR sensor is affected when the right IR sensor changes)
	 */
	public void test07() {
		RobowulfServo servoRight = new RobowulfServo(servoPins[1]);
		IRSensor irLeft = new IRSensor("Left IRSensor", irSensorPins[0]);
		IRSensor irRight = new IRSensor("Right IRSensor", irSensorPins[1]);
		
		servoRight.setBounds(1000, 2000);
		
		int result;

		IRSensorTestHelper helperRight = new IRSensorTestHelper(servoRight, irRight, irLeft);
		result = helperRight.doTest(10, 5000);

		servoRight.destroy();
		irLeft.destroy();
		irRight.destroy();
		
		assertFalse("The right IR sensor did not respond to wheel rotation. Ensure that the IR sensor is plugged into the correct pin, that the battery pack is turned on, and that the batteries are charged.",
					result == IRSensorTestHelper.FAILED_TIMEOUT);

		assertFalse("The left IR sensor's readings were affected by the right IR sensor. This is not supposed to happen, and may be the result of more than 3V being applied to the analog pin(s)",
					result == IRSensorTestHelper.FAILED_SENSOR_SWING || result == IRSensorTestHelper.FAILED_SENSOR_COUNTED);
	}

	public void test08() {
        USART
        CMUCam2 camera = new CMUCam2()
    }
	
	private boolean testIRSensor(IOutputPin servoPin, IScalarInput sensorPin) {
		RobowulfServo servo = new RobowulfServo(servoPin);
		IRSensor ir = new IRSensor("IRSensor", sensorPin);
		
		ir.resetCounter();
		
		servo.setBounds(1000, 2000);
		servo.forward();
		Utils.sleep(2000);
		servo.brake();
		Utils.sleep(250);
		servo.floatServo();
		Utils.sleep(250);
		
		boolean success = ir.getConvertedValue() > 0;
		
		servo.destroy();
		ir.destroy();
		
		return success;
	}
	
	private boolean testSonarOnPin(IIOPin pin) {
		MaxSonarEZ1UltrasoundSensor sonar1 = new MaxSonarEZ1UltrasoundSensor(pin, Units.INCHES);
		if (sonar1.testConnection()) {
			return true;
		}
		
		PINGUltrasoundSensor sonar2 = new PINGUltrasoundSensor(pin, Units.INCHES);
		if (sonar2.testConnection()) {
			return true;
		}
		
		return false;
	}
	
	private class IRSensorTestHelper implements IRSensorListener {
		public static final int SUCCESS = 0;
		public static final int FAILED_TIMEOUT = 1;
		public static final int FAILED_SENSOR_SWING = 2;
		public static final int FAILED_SENSOR_COUNTED = 3;

		private static final int SENSOR_SWING_THRESHOLD = 100;
		
		private int numCounters;
		private IRSensor thisSensor;
		private IRSensor otherSensor;
		private RobowulfServo thisServo;

		private int low;
		private int high;
		private boolean done;

		public IRSensorTestHelper(RobowulfServo thisServo, IRSensor thisSensor, IRSensor otherSensor) {
			this.thisSensor = thisSensor;
			this.thisServo = thisServo;
			this.otherSensor = otherSensor;
		}

		public synchronized void counterIncremented(IRSensor src, int counterVal) {
			if (done) return;
			
			try {
				int otherVal = (int)otherSensor.getRawValue();

				if (otherVal < low) low = otherVal;
				if (otherVal > high) high = otherVal;
			} catch (ResourceUnavailableException rue) {}

			if (counterVal == numCounters) {
				done = true;
				thisSensor.removeIRSensorListener(this);
				notifyAll();
			}
		}
		
		public synchronized int doTest(int numCounters, long timeout) {
			this.numCounters = numCounters;
			low = Integer.MAX_VALUE;
			high = Integer.MIN_VALUE;
			done = false;

			otherSensor.resetCounter();
			thisSensor.resetCounter();

			thisServo.forward();

			thisSensor.addIRSensorListener(this);

			try {
				wait(timeout);
			} catch (InterruptedException ie) {}

			thisSensor.removeIRSensorListener(this);
			thisServo.brake();
			Utils.sleep(250);
			thisServo.floatServo();
			Utils.sleep(250);
			
			if (!done) {
				done = true;
				return FAILED_TIMEOUT;
			}

			System.out.println("Swing: high=" + high + ", low=" + low);

			if (Math.abs(high - low) > SENSOR_SWING_THRESHOLD) {
				return FAILED_SENSOR_SWING;
			}

			System.out.println("This sensor counter: " + thisSensor.getConvertedValue());
			System.out.println("Other sensor counter: " + otherSensor.getConvertedValue());

			if (otherSensor.getConvertedValue() > 0) {
				return FAILED_SENSOR_COUNTED;
			}
			
			return SUCCESS;
		}
	}
}
